//
// Created by zhihui on 4/4/20.
//

#include <iostream>
#include <memory>
#include <cmath>

#include <rclcpp/rclcpp.hpp>
#include <visualization_msgs/msg/marker.hpp>

using namespace std::chrono_literals;

class ContactPublisher : public rclcpp::Node {
public:
    using Marker = visualization_msgs::msg::Marker;

    ContactPublisher()
            : Node("test") {
        publisher_ = this->create_publisher<Marker>("Marker", 10);
        shape_ = Marker::CUBE;

        timer_ = this->create_wall_timer(100ms, std::bind(&ContactPublisher::timer_callback, this));
    }

private:
    void timer_callback() {
        auto marker = Marker();
        Marker points, line_strip, line_list;
        points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
        points.header.stamp = line_strip.header.stamp = line_list.header.stamp = clock_.now();
        points.ns = line_strip.ns = line_list.ns = "points_and_lines";
        points.action = line_strip.action = line_list.action = Marker::ADD;
        points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

        points.id = 0;
        line_strip.id = 1;
        line_list.id = 2;

        points.type = Marker::POINTS;
        line_strip.type = Marker::LINE_STRIP;
        line_list.type = Marker::LINE_LIST;

        // POINTS markers use x and y scale for width/height respectively
        points.scale.x = 0.2;
        points.scale.y = 0.2;

        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
        line_strip.scale.x = 0.1;
        line_list.scale.x = 0.1;

        // Points are green
        points.color.g = 1.0f;
        points.color.a = 1.0;

        // Line strip is blue
        line_strip.color.b = 1.0;
        line_strip.color.a = 1.0;

        // Line list is red
        line_list.color.r = 1.0;
        line_list.color.a = 1.0;



        // Create the vertices for the points and lines
        for (uint32_t i = 0; i < 100; ++i) {
            float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
            float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

            geometry_msgs::msg::Point p;
            p.x = (int32_t) i - 50;
            p.y = y;
            p.z = z;

            points.points.push_back(p);
            line_strip.points.push_back(p);

            // The line list needs two points for each line
            line_list.points.push_back(p);
            p.z += 1.0;
            line_list.points.push_back(p);
        }

        publisher_->publish(points);
        publisher_->publish(line_strip);
        publisher_->publish(line_list);

        f += 0.04;
    }

    rclcpp::Publisher<Marker>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;

    rclcpp::Clock clock_;
    uint32_t shape_;
    float f = 0;
};


int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

    auto publisher_node = std::make_shared<ContactPublisher>();

    rclcpp::spin(publisher_node);

    return 0;
}